# 通过香橙派，使用python读取鑫精诚六维力传感器读数，XJC_EC108F

import pysoem
import struct
import time

def main():
    # 创建 EtherCAT 主站，打开指定网口（请确保 enp4s0 已正确配置）
    master = pysoem.Master()
    master.open('enp4s0')

    # 扫描从站设备
    if master.config_init() == 0:
        print("未发现任何从站设备")
        master.close()
        return

    sensor = master.slaves[0]
    print("发现传感器:", sensor.name)

    # 进入 PRE-OP 状态，此状态允许通过 SDO 访问设备对象字典
    master.state = pysoem.PREOP_STATE
    master.write_state()
    # 稍作延时确保状态切换成功
    time.sleep(0.1)

    print("开始持续读取 SDO 数据，按 Ctrl+C 停止...")

    # 循环读取每个通道数据
    indices = [0x6000, 0x6001, 0x6002, 0x6003, 0x6004, 0x6005]
    channel_names = ['Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz']

    try:
        while True:
            readings = {}
            for idx, name in zip(indices, channel_names):
                try:
                    # 读取对象 0x6000.1 ~ 0x6005.1
                    data = sensor.sdo_read(idx, 1)
                    # 根据文档，该数据类型为 REAL (32位 float)，使用小端格式解码
                    value = struct.unpack("<f", data)[0]
                    readings[name] = value
                except Exception as e:
                    readings[name] = f"Error: {e}"
            print("SDO 读取结果:", readings)
            time.sleep(0.001)  # 每 1ms 读取一次，可根据需要调整
    except KeyboardInterrupt:
        print("读取中断，程序退出")
    finally:
        master.close()

if __name__ == "__main__":
    main()